#include "bsp/bsp_drivers.h"
#include "bsp/cpu_ticks.h"
#include "hw_samples.h"
#include "os/os.h"
#include "os/types.h"
#include "utils/logger.h"

#define NB_OFFSET_SMAPLES_SHIFT (7)
#define NB_OFFSET_SAMPLES (1<<NB_OFFSET_SMAPLES_SHIFT)

void hw_sample_init(hw_samples_t *ph_curr) {
	adc_init();
	/*
	 * 留给adc 300us时间，把所有数据至少采集一遍到DMA，确保后续的获取adc数据的接口都能拿到正确的数据
	*/
#ifndef USE_MEX_CMD
	cpu_udelay(300);
#endif
	ph_curr->b_get_offset = false;
	ph_curr->adc_offset_a = CONFIG_ADC_FULL_MAX>>1;
	ph_curr->adc_offset_b = CONFIG_ADC_FULL_MAX>>1;
	ph_curr->adc_offset_c = CONFIG_ADC_FULL_MAX>>1;
	hw_sample_get_vbuscurr_offset(ph_curr);
}

void hw_sample_get_phacurr_offset(hw_samples_t *ph_curr){
	ph_curr->adc_offset_a = 0;
	ph_curr->adc_offset_b = 0;
	ph_curr->adc_offset_c = 0;

	ph_curr->sample_count = NB_OFFSET_SAMPLES;
	ph_curr->sample_skip = 16;

	ph_curr->b_get_offset = true;
}

void hw_sample_wait_phacurr_offset(hw_samples_t *ph_curr) {
	while(ph_curr->b_get_offset) {
		wdog_reload();
	}
	sys_debug("offset %d, %d, %d\n", ph_curr->adc_offset_a, ph_curr->adc_offset_b, ph_curr->adc_offset_c);
}


static s32 _round(s32 v1, s32 a) {
	s32 mod = v1 % a;
	if (mod * 2 >= a) {
		return (v1 / a + 1);
	}
	return v1/a;
}

bool hw_sample_calc_phacurr_offset(hw_samples_t *ph_curr) {
	if (!ph_curr->b_get_offset) {
		return false;
	}
	s16 phase_current1 = 0 , phase_current2 = 0;
	adc_get_phase_curr_value(0xFF, &phase_current1, &phase_current2);

	if (ph_curr->sample_skip > 0) {
		ph_curr->sample_skip --;
		return true;
	}
	if (ph_curr->sample_count > 0) {
		ph_curr->sample_count--;
#if CONFIG_PHASE_CURRENT_PHASE_AB
		ph_curr->adc_offset_a += phase_current1;
		ph_curr->adc_offset_b += phase_current2;
		if (ph_curr->sample_count == 0) {
			ph_curr->adc_offset_a = _round(ph_curr->adc_offset_a, NB_OFFSET_SAMPLES);
			ph_curr->adc_offset_b = _round(ph_curr->adc_offset_b, NB_OFFSET_SAMPLES);
		}
#elif CONFIG_PHASE_CURRENT_PHASE_AC
		ph_curr->adc_offset_a += phase_current1;
		ph_curr->adc_offset_c += phase_current2;
		if (ph_curr->sample_count == 0) {
			ph_curr->adc_offset_a = _round(ph_curr->adc_offset_a, NB_OFFSET_SAMPLES);
			ph_curr->adc_offset_c = _round(ph_curr->adc_offset_c, NB_OFFSET_SAMPLES);
		}
#else
		ph_curr->adc_offset_b += phase_current1;
		ph_curr->adc_offset_c += phase_current2;
		if (ph_curr->sample_count == 0) {
			ph_curr->adc_offset_b = _round(ph_curr->adc_offset_b, NB_OFFSET_SAMPLES);
			ph_curr->adc_offset_c = _round(ph_curr->adc_offset_c, NB_OFFSET_SAMPLES);
		}
#endif
	}else {
		ph_curr->b_get_offset = false;
	}
	return true;
}


bool hw_sample_check_phacurr_offset(hw_samples_t *ph_curr) {
#if CONFIG_PHASE_CURRENT_PHASE_AB
	if ((ph_curr->adc_offset_a > (CONFIG_ADC_FULL_MAX>>1) + CONFIG_ADC_OFFSET_TOLERANCE) || (ph_curr->adc_offset_b > (CONFIG_ADC_FULL_MAX>>1) + CONFIG_ADC_OFFSET_TOLERANCE)) {
		return true;
	}
	if ((ph_curr->adc_offset_a < (CONFIG_ADC_FULL_MAX>>1) - CONFIG_ADC_OFFSET_TOLERANCE) || (ph_curr->adc_offset_b < (CONFIG_ADC_FULL_MAX>>1) - CONFIG_ADC_OFFSET_TOLERANCE)) {
		return true;
	}
#elif CONFIG_PHASE_CURRENT_PHASE_BC
	if ((ph_curr->adc_offset_c > (CONFIG_ADC_FULL_MAX>>1) + CONFIG_ADC_OFFSET_TOLERANCE) || (ph_curr->adc_offset_b > (CONFIG_ADC_FULL_MAX>>1) + CONFIG_ADC_OFFSET_TOLERANCE)) {
		return true;
	}
	if ((ph_curr->adc_offset_c < (CONFIG_ADC_FULL_MAX>>1) - CONFIG_ADC_OFFSET_TOLERANCE) || (ph_curr->adc_offset_b < (CONFIG_ADC_FULL_MAX>>1) - CONFIG_ADC_OFFSET_TOLERANCE)) {
		return true;
	}
#elif CONFIG_PHASE_CURRENT_PHASE_AC
	if ((ph_curr->adc_offset_c > (CONFIG_ADC_FULL_MAX>>1) + CONFIG_ADC_OFFSET_TOLERANCE) || (ph_curr->adc_offset_a > (CONFIG_ADC_FULL_MAX>>1) + CONFIG_ADC_OFFSET_TOLERANCE)) {
		return true;
	}
	if ((ph_curr->adc_offset_c < (CONFIG_ADC_FULL_MAX>>1) - CONFIG_ADC_OFFSET_TOLERANCE) || (ph_curr->adc_offset_a < (CONFIG_ADC_FULL_MAX>>1) - CONFIG_ADC_OFFSET_TOLERANCE)) {
		return true;
	}
#endif

	return false;
}

#define adc_to_current(adc) (adc * CONFIG_CURRENT_SENSOR_CEOF * CONFIG_CURRENT_POLARITY)

void hw_sample_get_phacurr(hw_samples_t *ph_curr, float *iABC){

	s16 phase1, phase2;
	adc_get_phase_curr_value(0xFF, &phase1, &phase2);

#if CONFIG_PHASE_CURRENT_PHASE_AB
	s16 adc_ia = (phase1 - ph_curr->adc_offset_a);
	s16 adc_ib = (phase2 - ph_curr->adc_offset_b);
	iABC[0] = adc_to_current(adc_ia);
	iABC[1] = adc_to_current(adc_ib);
	iABC[2] = -(iABC[0] + iABC[1]);
#elif CONFIG_PHASE_CURRENT_PHASE_AC
	s16 adc_ia = (phase1 - ph_curr->adc_offset_a);
	s16 adc_ic = (phase2 - ph_curr->adc_offset_c);
	iABC[0] = adc_to_current(adc_ia);
	iABC[2] = adc_to_current(adc_ic);
	iABC[1] = -(iABC[0] + iABC[2]);
#else
	s16 adc_ib = (phase1 - ph_curr->adc_offset_b);
	s16 adc_ic = (phase2 - ph_curr->adc_offset_c);
	iABC[1] = adc_to_current(adc_ib);
	iABC[0] = adc_to_current(adc_ic);
	iABC[2] = -(iABC[1] + iABC[0]);
#endif
}


bool hw_sample_get_vbuscurr_offset(hw_samples_t *samples) {
#if (CONFIG_DC_CURR_SAMPLE)
#if defined(USE_MEX_CMD)
	samples->adc_offset_ibus = 2048;
	return true;
#else
	int count = 0;
	s32 sum = 0;
	cpu_udelay(200); // wait for first adc complete
	while(count++ < 16) {
		sum += adc_get_dc_current_value();
		cpu_udelay(200);
	}
	//sys_debug("ibus adc sum %d, %d\n", sum, adc_get_dc_current_value());
	samples->adc_offset_ibus = sum / (count-1);
	if (samples->adc_offset_ibus < (CONFIG_ADC_FULL_MAX/2 - CONFIG_ADC_OFFSET_TOLERANCE) || 
		samples->adc_offset_ibus > (CONFIG_ADC_FULL_MAX/2 + CONFIG_ADC_OFFSET_TOLERANCE)) {
		return false;
	}
	return true;
#endif
#else
	return true;
#endif
}

float hw_sample_get_vbus_curr(hw_samples_t *samples) {
#if (CONFIG_DC_CURR_SAMPLE)
	s16 adc = adc_get_dc_current_value() - samples->adc_offset_ibus;
	return (adc * CONFIG_IBUS_SENSOR_CEOF * CONFIG_DC_CURR_POLARITY);
#else
	return 0;
#endif
}

float hw_sample_get_vbus_vol(hw_samples_t *samples) {
	s16 adc = adc_get_dc_voltage_value();
	return (adc * CONFIG_VBUS_SENSOR_CEOF);
}


